#ifndef OMPL_PLANNER_H
#define OMPL_PLANNER_H

#include <iostream>
#include <ros/ros.h>

#include <ompl/base/OptimizationObjective.h>
#include <ompl/base/objectives/PathLengthOptimizationObjective.h>
#include <ompl/base/objectives/StateCostIntegralObjective.h>
#include <ompl/base/spaces/SE3StateSpace.h>
#include <ompl/config.h>
#include <ompl/geometric/PathSimplifier.h>
#include <ompl/geometric/planners/rrt/InformedRRTstar.h>
#include <visualization_msgs/Marker.h>

#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Geometry>

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/kdtree/kdtree_flann.h>

namespace Planner
{
    using Eigen::Vector3d;
    using std::cout;
    using std::endl;
    using std::vector;
    namespace ob = ompl::base;
    namespace og = ompl::geometric;

    // 障碍物间隙项目标函数
    class ClearanceObjective : public ob::StateCostIntegralObjective
    {
    public:
        ClearanceObjective(const ob::SpaceInformationPtr &si)
            : ob::StateCostIntegralObjective(si, true) {}

        ob::Cost stateCost(const ob::State *s) const
        {
            // double clearance = si_->getStateValidityChecker()->clearance(s);
            // double cost;
            // if (clearance < 0.01)
            //     cost = 100.0;
            // else
            //     cost = 1 / clearance;
            // return ob::Cost(cost);
            return ob::Cost(1 / si_->getStateValidityChecker()->clearance(s));
        }
    };

    // 碰撞检测器
    class ValidityChecker : public ob::StateValidityChecker
    {
    public:
        ValidityChecker(const ob::SpaceInformationPtr &si) : ob::StateValidityChecker(si) {}

        bool isValid(const ob::State *state) const
        {
            return this->clearance(state) > 0.0;
        }

        // 障碍物间隙
        double clearance(const ob::State *state) const;

        void setMap(const pcl::KdTreeFLANN<pcl::PointXYZ>::Ptr &tree)
        {
            kdtree_ = tree;
        }

        void setSecureDis(double dis)
        {
            secure_dis_ = dis;
        }

        double calcDis(const double &x, const double &y, const double &z) const;

    private:
        pcl::KdTreeFLANN<pcl::PointXYZ>::Ptr kdtree_;
        double secure_dis_;
    };

    class OMPLPlanner
    {
    public:
        bool setStart(const Vector3d &pt);

        bool setGoal(const Vector3d &pt);

        void setCloudMap(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud, pcl::KdTreeFLANN<pcl::PointXYZ>::Ptr &tree);

        void setParams(const ros::NodeHandle &nh);

        bool plan();

        vector<Vector3d> getPath(bool smooth = false);

    private:
        ob::StateSpacePtr space_;
        ob::SpaceInformationPtr si_;
        ob::ProblemDefinitionPtr pdef_;
        std::shared_ptr<ValidityChecker> checker_;
        std::shared_ptr<og::PathGeometric> path_;

        double x_min_, x_max_, y_min_, y_max_, z_min_, z_max_; // 搜索范围
        double solve_time_;                                    // 求解时间
        double rrt_gap_;                                       // 搜索步长
        double secure_dis_;                                    // 安全距离
        bool dynamic_configure_;                               // 是否动态配置参数
        double dynamic_gap_;                                   // 动态配置边界的间隙
        bool print_info_;                                      // 是否输出求解信息
        int opt_func_;                                         // 0:最短距离 1:综合目标
        double wc_, wl_;                                       // 加权目标函数权重

        Vector3d start_pos_, goal_pos_;

        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_map_;
        pcl::KdTreeFLANN<pcl::PointXYZ>::Ptr kdtree_;

        // 最短路径优化指标
        ob::OptimizationObjectivePtr getPathLengthObjWithCostToGo(const ob::SpaceInformationPtr &si);

        // 路径长度与障碍物间隙加权优化指标
        ob::OptimizationObjectivePtr getBalancedObjective(const ob::SpaceInformationPtr &si);

        // 动态配置搜索范围，时间等参数
        void dynamicInitParams();
    };
} // namespace Planner

#endif